#ifndef PLANNER_PKG__GLOBAL_DIJKSTRA_PLANNER_HPP_
#define PLANNER_PKG__GLOBAL_DIJKSTRA_PLANNER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "rclcpp/rclcpp.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav_msgs/msg/odometry.hpp"
// 自定义
#include "planner_pkg/dijkstra.hpp"

namespace planner_pkg
{
    // 自定义导航规划器类
    class GlobalDijkstraPlanner : public nav2_core::GlobalPlanner
    {
    public:
        GlobalDijkstraPlanner();
        ~GlobalDijkstraPlanner();
        // 插件配置方法
        void configure(
            const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
            std::string name,
            std::shared_ptr<tf2_ros::Buffer> tf,
            std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
        // 插件清理方法
        void cleanup() override;
        // 插件激活方法
        void activate() override;
        // 插件停用方法
        void deactivate() override;
        // 为给定的起始和目标位姿创建路径的方法
        nav_msgs::msg::Path createPlan(const geometry_msgs::msg::PoseStamped &start,
                                       const geometry_msgs::msg::PoseStamped &goal) override;

    protected:
        bool makePlan(
            const geometry_msgs::msg::Pose &start,
            const geometry_msgs::msg::Pose &goal,
            nav_msgs::msg::Path &plan);

        /**
         * @brief Compute the squared distance between two points
         * @param p1 Point 1
         * @param p2 Point 2
         * @return double squared distance between two points
         */
        inline double squared_distance(
            const geometry_msgs::msg::Pose &p1,
            const geometry_msgs::msg::Pose &p2)
        {
            double dx = p1.position.x - p2.position.x;
            double dy = p1.position.y - p2.position.y;
            return dx * dx + dy * dy;
        }

        /**
         * @brief Remove artifacts at the end of the path - originated from planning on a discretized world
         * @param goal Goal pose
         * @param plan Computed path
         */
        void smoothApproachToGoal(
            const geometry_msgs::msg::Pose &goal,
            nav_msgs::msg::Path &plan);

    private:
        // 坐标变换缓存指针，可用于查询坐标关系
        std::shared_ptr<tf2_ros::Buffer> tf_;
        // 节点指针
        nav2_util::LifecycleNode::SharedPtr node_;
        // 全局代价地图
        nav2_costmap_2d::Costmap2D *costmap_;
        // 全局代价地图的坐标系
        std::string global_frame_, name_;

        // Clock
        rclcpp::Clock::SharedPtr clock_;
        // Logger
        rclcpp::Logger logger_{rclcpp::get_logger("Dijkstra")};

        std::unique_ptr<Dijkstra> planner_;

        bool allow_unknown_;
        double tolerance_;
    };

} // namespace planner_pkg

#endif // PLANNER_PKG__GLOBAL_DIJKSTRA_PLANNER_HPP_